#include "HeaderFile/SerialPort.hpp"
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <dirent.h>
#include <algorithm>

SerialPort::SerialPort() 
    : m_fd(-1), m_stopReading(false) {
}

SerialPort::~SerialPort() {
    close();
}

bool SerialPort::open(const Config& config) {
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (isOpen()) {
        setError("串口已经打开");
        return false;
    }

    m_config = config;
    
    // 打开串口设备
    m_fd = ::open(config.device.c_str(), O_RDWR | O_NOCTTY);
    if (m_fd == -1) {
        setError("无法打开串口设备: " + config.device + " - " + std::string(strerror(errno)));
        return false;
    }

    // // 配置串口参数
    if (!configurePort()) {
        ::close(m_fd);
        m_fd = -1;
        return false;
    }

    // 清空输入输出缓冲区
    tcflush(m_fd, TCIOFLUSH);
    
    return true;
}

void SerialPort::close() {
    // 停止异步读取
    stopAsyncRead();
    std::lock_guard<std::mutex> lock(m_mutex);
    std::cout << "关闭串口" << std::endl;
    if (m_fd != -1) {
        ::close(m_fd);
        m_fd = -1;
    }
}

bool SerialPort::isOpen() const {
    return m_fd != -1;
}

// 在 SerialPort 类中添加快速写入方法（无锁版本，用于回调中）
// 注意：这个方法假设串口已经打开且不会被其他线程关闭
bool SerialPort::writeFast(const std::vector<uint8_t>& data) {
    if (m_fd == -1 || data.empty()) {
        return false;
    }
    
    // 直接写入，不加锁（用于回调函数中，提高性能）
    ssize_t bytesWritten = ::write(m_fd, data.data(), data.size());
    return (bytesWritten == static_cast<ssize_t>(data.size()));
}

// 普通写入不带校验和
bool SerialPort::write(const std::vector<uint8_t>& data) {
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!isOpen()) {
        setError("串口未打开");
        return false;
    }

    if (data.empty()) {
        return true;
    }

    ssize_t bytesWritten = ::write(m_fd, data.data(), data.size());
    if (bytesWritten == -1) {
        setError("写入数据失败: " + std::string(strerror(errno)));
        return false;
    }

    if (static_cast<size_t>(bytesWritten) != data.size()) {
        setError("写入数据不完整");
        return false;
    }

    return true;
}

// 带校验和的写入
bool SerialPort::writeWithChecksum(std::vector<uint8_t>& data){

    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!isOpen()) {
        setError("串口未打开");
        return false;
    }

    if (data.empty()) {
        return true;
    }

    // 加校验位
    std::vector<uint8_t> dataWithChecksum = data;
    uint8_t checksum = calculateChecksum(data);
    dataWithChecksum.push_back(checksum);

    ssize_t bytesWritten = ::write(m_fd, dataWithChecksum.data(), dataWithChecksum.size());
    if (bytesWritten == -1) {
        setError("写入数据失败: " + std::string(strerror(errno)));
        return false;
    }

    if (static_cast<size_t>(bytesWritten) != dataWithChecksum.size()) {
        setError("写入数据不完整");
        return false;
    }

    return true;
}

bool SerialPort::write(const std::string& data) {
    std::vector<uint8_t> buffer(data.begin(), data.end());
    return write(buffer);
}

std::vector<uint8_t> SerialPort::read(size_t maxBytes) {
    std::lock_guard<std::mutex> lock(m_mutex);
    
    std::vector<uint8_t> buffer;
    
    if (!isOpen()) {
        setError("串口未打开");
        return buffer;
    }

    buffer.resize(maxBytes);
    
    // 设置读取超时
    fd_set readfds;
    struct timeval timeout;
    
    FD_ZERO(&readfds);
    FD_SET(m_fd, &readfds);
    
    timeout.tv_sec = m_config.timeout / 1000;
    timeout.tv_usec = (m_config.timeout % 1000) * 1000;
    
    int result = select(m_fd + 1, &readfds, nullptr, nullptr, &timeout);
    
    if (result == -1) {
        setError("读取数据时select失败: " + std::string(strerror(errno)));
        buffer.clear();
        return buffer;
    }
    
    if (result == 0) {
        // 超时
        buffer.clear();
        return buffer;
    }
    
    ssize_t bytesRead = ::read(m_fd, buffer.data(), maxBytes);
    if (bytesRead == -1) {
        setError("读取数据失败: " + std::string(strerror(errno)));
        buffer.clear();
        return buffer;
    }
    
    buffer.resize(bytesRead);
    return buffer;
}

std::string SerialPort::readString(size_t maxBytes) {
    auto data = read(maxBytes);
    return std::string(data.begin(), data.end());
}

void SerialPort::startAsyncRead(DataCallback callback) {
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!isOpen()) {
        setError("串口未打开");
        return;
    }
    
    if (m_readThread.joinable()) {
        setError("异步读取已经在运行");
        return;
    }
    
    m_dataCallback = callback;
    m_stopReading = false;
    m_readThread = std::thread(&SerialPort::readThreadFunction, this);
}

void SerialPort::stopAsyncRead() {
    m_stopReading = true;
    if (m_readThread.joinable()) {
        m_readThread.join();
    }
}

std::vector<std::string> SerialPort::listAvailablePorts() {
    std::vector<std::string> ports;
    
    // 在Linux系统中查找串口设备
    const std::vector<std::string> searchPaths = {
        "/dev/ttyS",    // 标准串口
        "/dev/ttyUSB",  // USB串口
        "/dev/ttyACM",  // ACM设备
        "/dev/ttyTHS"   // Tegra串口
    };
    
    for (const auto& basePath : searchPaths) {
        for (int i = 0; i < 32; ++i) {
            std::string device = basePath + std::to_string(i);
            
            // 尝试打开设备以检查是否存在
            int fd = ::open(device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
            if (fd != -1) {
                ports.push_back(device);
                ::close(fd);
            }
        }
    }
    
    // 排序端口列表
    std::sort(ports.begin(), ports.end());
    
    return ports;
}

std::string SerialPort::getLastError() const {
    std::lock_guard<std::mutex> lock(m_mutex);
    return m_lastError;
}

bool SerialPort::configurePort() {
    struct termios options;
    
    // 获取当前串口配置
    if (tcgetattr(m_fd, &options) != 0) {
        setError("获取串口配置失败: " + std::string(strerror(errno)));
        return false;
    }
    
    // 设置波特率
    speed_t baudRate;
    switch (m_config.baudRate) {
        case 9600:   baudRate = B9600; break;
        case 19200:  baudRate = B19200; break;
        case 38400:  baudRate = B38400; break;
        case 57600:  baudRate = B57600; break;
        case 115200: baudRate = B115200; break;
        case 230400: baudRate = B230400; break;
        case 460800: baudRate = B460800; break;
        case 921600: baudRate = B921600; break;
        default:
            setError("不支持的波特率: " + std::to_string(m_config.baudRate));
            return false;
    }
    
    cfsetispeed(&options, baudRate);
    cfsetospeed(&options, baudRate);
    
    // 设置数据位
    options.c_cflag &= ~CSIZE;
    switch (m_config.dataBits) {
        case 5: options.c_cflag |= CS5; break;
        case 6: options.c_cflag |= CS6; break;
        case 7: options.c_cflag |= CS7; break;
        case 8: options.c_cflag |= CS8; break;
        default:
            setError("不支持的数据位: " + std::to_string(m_config.dataBits));
            return false;
    }
    
    // 设置停止位
    if (m_config.stopBits == 1) {
        options.c_cflag &= ~CSTOPB;
    } else if (m_config.stopBits == 2) {
        options.c_cflag |= CSTOPB;
    } else {
        setError("不支持的停止位: " + std::to_string(m_config.stopBits));
        return false;
    }
    
    // 设置校验位
    switch (m_config.parity) {
        case 'N':
        case 'n':
            options.c_cflag &= ~PARENB;
            break;
        case 'E':
        case 'e':
            options.c_cflag |= PARENB;
            options.c_cflag &= ~PARODD;
            break;
        case 'O':
        case 'o':
            options.c_cflag |= PARENB;
            options.c_cflag |= PARODD;
            break;
        default:
            setError("不支持的校验位: " + std::string(1, m_config.parity));
            return false;
    }
    
    // 设置控制模式
    options.c_cflag |= CLOCAL | CREAD;  // 忽略调制解调器状态，启用接收
    options.c_cflag &= ~CRTSCTS;    // 禁用硬件流控制
    
    // 设置输入模式
    options.c_iflag &= ~(IXON | IXOFF | IXANY);  // 禁用软件流控制
    options.c_iflag &= ~(INLCR | ICRNL);         // 禁用换行符转换
    
    // 设置输出模式
    options.c_oflag &= ~OPOST;  // 禁用输出处理
    
    // 设置本地模式
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // 原始模式
    
    // 设置读取参数
    options.c_cc[VMIN] = 0;   // 最小00读取字符数
    options.c_cc[VTIME] = 1;  // 读取超时(0.1秒单位)
    
    // 应用配置
    if (tcsetattr(m_fd, TCSANOW, &options) != 0) {
        setError("设置串口配置失败: " + std::string(strerror(errno)));
        return false;
    }
    
    return true;
}

void SerialPort::readThreadFunction() {
    const size_t bufferSize = m_config.bufferSize;
    std::vector<uint8_t> buffer(bufferSize);
    
    while (!m_stopReading && isOpen()) {
        fd_set readfds;
        timeval timeout{};
        FD_ZERO(&readfds);
        FD_SET(m_fd, &readfds);
        timeout.tv_sec = 0;
        timeout.tv_usec = 100000;
        int result = select(m_fd + 1, &readfds, nullptr, nullptr, &timeout);
        if (result == -1) {
            if (errno != EINTR) {
                setError("select() 失败: " + std::string(strerror(errno)));
                break;
            }
            continue;
        }
    
        if (result > 0 && FD_ISSET(m_fd, &readfds)) {
            ssize_t bytesRead = ::read(m_fd, buffer.data(), bufferSize);
            if (bytesRead > 0) {
                std::vector<uint8_t> data(buffer.begin(), buffer.begin() + bytesRead);
                if (m_dataCallback) m_dataCallback(data);
            } else if (bytesRead == 0) {
                setError("串口已关闭 (EOF)");
                break;
            } else {
                if (errno != EAGAIN && errno != EWOULDBLOCK) {
                    setError("read() 失败: " + std::string(strerror(errno)));
                    break;
                }
            }
        }
    
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

void SerialPort::setError(const std::string& error) {
    m_lastError = error;
    std::cerr << "SerialPort Error: " << error << std::endl;
}

// 将所有字节的数据值相加，然后对 256 取模的函数
uint8_t SerialPort::calculateChecksum(std::vector<uint8_t>& data) {
    uint32_t sum = 0;
    for (uint8_t byte : data) {
        sum += byte;
    }
    return static_cast<uint8_t>(sum % 256);
}


// 验证校验和的函数
bool SerialPort::verifyChecksum(std::vector<uint8_t>& data, uint8_t expectedChecksum) {
    return calculateChecksum(data) == expectedChecksum; 
}
// 将多个16位值转换为小端序字节数组
std::vector<uint8_t> SerialPort::uint16ArrayToLittleEndian(const std::vector<uint16_t>& values) {
    std::vector<uint8_t> result;
    for (uint16_t value : values) {
        result.push_back(value & 0xFF);        // 低字节
        result.push_back((value >> 8) & 0xFF); // 高字节
    }
    return result;
}
/**
解释
SerialPort::GyroFrame                    // 返回类型
SerialPort::parseGyroFrame              // 函数名（包含类名）
(const std::vector<uint8_t>& frame)     // 参数
陀螺仪数据
*/
SerialPort::GyroFrame SerialPort::parseGyroFrame(const std::vector<uint8_t>& frame) {
    GyroFrame result;

    // if (frame.size() != 11) { // 固定 11 字节
    //     setError("陀螺仪帧长度错误");
    //     return result;
    // }

    if (frame[0] != 0x52 || frame[1] != 0x0B) {
        setError("陀螺仪帧头错误");
        return result;
    }

    // 计算 CRC（帧尾 CRC 覆盖前 10 个字节）
    std::vector<uint8_t> payload(frame.begin(), frame.begin() + 10);
    uint8_t crc = calculateChecksum(payload);
    if (crc == frame[10]) {
        result.valid = true;
    } else {
        setError("陀螺仪帧 CRC 校验失败");
    }
    

    // 提取数据（大端高位在前）
    auto readFloat = [](uint8_t high, uint8_t low) -> float {
        return ( static_cast<int16_t>((high << 8) | low) ) / 100.0f; // 转换为度
    };
    auto readUint16 = [](uint8_t high, uint8_t low) -> uint16_t {
        return static_cast<uint16_t>((high << 8) | low);
    };

    result.roll  = readFloat(frame[2], frame[3]);
    result.pitch = readFloat(frame[4], frame[5]);
    result.yaw   = readFloat(frame[6], frame[7]);
    result.tick  = readUint16(frame[8], frame[9]);

    return result;
}